#include "Camera.h"
#include "Objects.h"

#include "TrackAPI.h"
#include "AxisCamera.h"
#include "string.h"
#include "vxWorks.h" 
#include "AxisCamera.h" 
#include "FrcError.h"
#include "Utility.h" 
#include "Task.h"

namespace CameraLib {

Camera::Camera(TrackingThreshold primaryColor, TrackingThreshold secondaryColor, MyOrder foe){
	m_primaryColor = primaryColor;
	m_secondaryColor = secondaryColor;
	m_foe = foe;
	if (StartCameraTask(13, 0, k160x120, ROT_0) == -1) {
			printf("Failed to spawn camera task; exiting. Error code %s\n", 
					GetVisionErrorText(GetLastVisionError()) );
		}
	Priv_SetWriteFileAllowed(1);
}

void Camera::Calculate() {
	flags = FindFlags(m_primaryColor, m_secondaryColor, m_foe);
}

void Camera::Thread() {
	//Task t = new Task('a', Camera::Run);
	//t.Run();
}

void Camera::Run() {
	while (true) Calculate();
}

std::list<Object> Camera::FindColor(TrackingThreshold td) {
	//TODO: raise: if (!cameraImage) {return NULL;}
	
	//TODO: raise: if (!success) "No camera Image available Error = %i %s"

	Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
	if (!cameraImage) {printf("Camera Fail\n");}
	if (!GetImage(cameraImage, NULL)) {printf("GetImage FAIL\n");}
	
	int numParticles, success;
	std::list<Object> *l_objects = new std::list<Object>();

	success = frcColorThreshold(&*cameraImage, &*cameraImage, IMAQ_HSL, &td.hue, &td.saturation, &td.luminance);
	
	success = frcCountParticles(cameraImage, &numParticles);
	if (!success) {printf("Particle count fail\n");}
	
	for (int i = 0; i < numParticles; ++i) {
		double x, y, height, width;
		imaqMeasureParticle(cameraImage, i, 0, IMAQ_MT_CENTER_OF_MASS_X, &x);
		imaqMeasureParticle(cameraImage, i, 0, IMAQ_MT_CENTER_OF_MASS_Y, &y);
		imaqMeasureParticle(cameraImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
		imaqMeasureParticle(cameraImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
		
		if (width*height >= (double) MINIMUM_AREA) {
			l_objects->push_back(Object((float) (x - 80) / 80, (float) (y - 60) / 60, (float) width / 80, (float) height / 60));
		}
	}
	frcDispose("FindFlags", cameraImage, NULL);
	return *l_objects;
}

std::list<Flag> Camera::FindFlags(TrackingThreshold green, TrackingThreshold pink, MyOrder me) {
	//Timer* tt = new Timer();
	//tt->Start();
	timeStamp = GetTime();
	std::list<Object> greenBlobs, pinkBlobs; 
	std::list<Flag> f;
	greenBlobs = FindColor(green);
	pinkBlobs = FindColor(pink);
	objects = pinkBlobs;
	for (ITERATOR(Object) green = greenBlobs.begin(); green != greenBlobs.end(); green++) {
		bool to_break = false;
		for (ITERATOR(Object) pink = pinkBlobs.begin(); pink != pinkBlobs.end(); pink++) {
			MyOrder oth;
			if (IsFlag(*green, *pink)) {
				to_break = true;
				if (green->GetY() > pink->GetY()) oth = GREENPINK;
				else oth = PINKGREEN;
				f.push_back(Flag(*green, *pink, me, oth));
				break;
			}
		}
		if (to_break) break;
	}/*
	for (ITERATOR(Object) green = greenBlobs.begin(); green != greenBlobs.end(); green++) {
		printf("Green: "); (*green).print();
	}
	for (ITERATOR(Object) pink = pinkBlobs.begin(); pink != pinkBlobs.end(); pink++) {
		printf("Pink: "); (*pink).print();
	}*/
	//tt->Stop();
	//printf("FindFlags took %e seconds.\n", tt->Get());
	return f;
}

bool Camera::IsFlag(Object green, Object pink) {
	if ((green.GetDistanceFrom(pink) <= DISTANCE_THRESHOLD * green.GetHeight()) &&
		(pink.GetArea() * EQUAL_NEG_THRESHOLD <= green.GetArea() && green.GetArea() <= pink.GetArea() * EQUAL_THRESHOLD))
		return true;
	else
		return false;
}

}
